{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# EssentialMatrixFactor Variants"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines several factors related to the Essential matrix ($E$), which encodes the relative rotation and translation direction between two *calibrated* cameras.\n",
        "They are primarily used in Structure from Motion (SfM) problems where point correspondences between views are available but the 3D structure and/or camera poses/calibration are unknown.\n",
        "\n",
        "The core constraint is the epipolar constraint: $p_2^T E p_1 = 0$, where $p_1$ and $p_2$ are corresponding points in *normalized (calibrated)* image coordinates.\n",
        "\n",
        "Factors defined here:\n",
        "*   `EssentialMatrixFactor`: Constrains an unknown `EssentialMatrix` variable using a single point correspondence $(p_1, p_2)$.\n",
        "*   `EssentialMatrixFactor2`: Constrains an `EssentialMatrix` and an unknown inverse depth variable using a point correspondence.\n",
        "*   `EssentialMatrixFactor3`: Like Factor2, but incorporates an additional fixed extrinsic rotation (useful for camera rigs).\n",
        "*   `EssentialMatrixFactor4<CALIBRATION>`: Constrains an `EssentialMatrix` and a *shared* `CALIBRATION` variable using a point correspondence given in *pixel* coordinates.\n",
        "*   `EssentialMatrixFactor5<CALIBRATION>`: Constrains an `EssentialMatrix` and *two* unknown `CALIBRATION` variables (one for each camera) using a point correspondence given in *pixel* coordinates."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import (EssentialMatrix, Point2, Point3, Rot3, Unit3, Pose3, Cal3_S2, EssentialMatrixFactor,\n",
        "                   EssentialMatrixFactor2, EssentialMatrixFactor3, EssentialMatrixFactor4Cal3_S2,\n",
        "                   EssentialMatrixFactor5Cal3_S2, Values)\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "E = symbol_shorthand.E\n",
        "K = symbol_shorthand.K\n",
        "D = symbol_shorthand.D # For inverse depth"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_header_md"
      },
      "source": [
        "## 1. `EssentialMatrixFactor`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_desc_md"
      },
      "source": [
        "This factor involves a single `EssentialMatrix` variable. It takes a pair of corresponding points $(p_A, p_B)$ in *normalized (calibrated)* image coordinates and penalizes deviations from the epipolar constraint $p_B^T E p_A = 0$.\n",
        "The error is $p_B^T E p_A$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "factor1_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Factor 1:   keys = { e0 }\n",
            "isotropic dim=1 sigma=0.01\n",
            "  EssentialMatrixFactor with measurements\n",
            "  (0.5 0.2   1)' and ( 0.4 0.25    1)'\n",
            "\n",
            "Error for Factor 1: 12.499999999999995\n"
          ]
        }
      ],
      "source": [
        "# Assume normalized coordinates\n",
        "pA_calibrated = Point2(0.5, 0.2)\n",
        "pB_calibrated = Point2(0.4, 0.25)\n",
        "\n",
        "# Noise model on the epipolar error (scalar)\n",
        "epipolar_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)\n",
        "\n",
        "# Key for the unknown Essential Matrix\n",
        "keyE = E(0)\n",
        "\n",
        "factor1 = EssentialMatrixFactor(keyE, pA_calibrated, pB_calibrated, epipolar_noise)\n",
        "factor1.print(\"Factor 1: \")\n",
        "\n",
        "# Evaluate error (requires an EssentialMatrix value)\n",
        "values = Values()\n",
        "# Example: E for identity rotation, translation (1,0,0)\n",
        "example_E = EssentialMatrix(Rot3(), Unit3(1, 0, 0))\n",
        "values.insert(keyE, example_E)\n",
        "error1 = factor1.error(values)\n",
        "print(f\"\\nError for Factor 1: {error1}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_header_md"
      },
      "source": [
        "## 2. `EssentialMatrixFactor2`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_desc_md"
      },
      "source": [
        "This factor involves an `EssentialMatrix` variable and a `double` variable representing the *inverse depth* of the 3D point corresponding to the measurement $p_A$ in the first camera's frame.\n",
        "It assumes the measurement $p_B$ is perfect and calculates the reprojection error of the point (reconstructed using $p_A$ and the inverse depth) in the first camera image, after projecting it into the second camera and back.\n",
        "It requires point correspondences $(p_A, p_B)$, which can be provided in either calibrated or pixel coordinates (if a calibration object `K` is provided).\n",
        "The error is a 2D reprojection error in the first image plane (typically in pixels if K is provided)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "id": "factor2_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Factor 2:   keys = { e0 d0 }\n",
            "  noise model: unit (2) \n",
            "  EssentialMatrixFactor2 with measurements\n",
            "  (480 288   1)' and (464 312)'\n",
            "\n",
            "Error for Factor 2: 412.82000000000016\n"
          ]
        }
      ],
      "source": [
        "# Assume pixel coordinates and known calibration\n",
        "K_cal = Cal3_S2(500, 500, 0, 320, 240)\n",
        "pA_pixels = Point2(480, 288) # Corresponds to (0.5, 0.2) calibrated\n",
        "pB_pixels = Point2(464, 312) # Corresponds to (0.4, 0.25) calibrated\n",
        "\n",
        "# Noise model on the 2D reprojection error (pixels)\n",
        "reprojection_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "\n",
        "# Key for inverse depth\n",
        "keyD = D(0)\n",
        "\n",
        "factor2 = EssentialMatrixFactor2(keyE, keyD, pA_pixels, pB_pixels, reprojection_noise)\n",
        "factor2.print(\"\\nFactor 2: \")\n",
        "\n",
        "# Evaluate error (requires E and inverse depth d)\n",
        "values.insert(keyD, 0.2) # Assume inverse depth d = 1/Z = 1/5 = 0.2\n",
        "error2 = factor2.error(values)\n",
        "print(f\"\\nError for Factor 2: {error2}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor3_header_md"
      },
      "source": [
        "## 3. `EssentialMatrixFactor3`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor3_desc_md"
      },
      "source": [
        "This is identical to `EssentialMatrixFactor2` but includes an additional fixed `Rot3` representing the rotation from the 'body' frame (where the Essential matrix is defined) to the 'camera' frame (where the measurements are made).\n",
        "`iRc`: Rotation from camera frame to body frame (inverse of body-to-camera).\n",
        "The Essential matrix $E_{body}$ is transformed to the camera frame before use: $E_{camera} = R_{cRb} \\cdot E_{body}$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "metadata": {
        "id": "factor3_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Factor 3:   keys = { e0 d0 }\n",
            "  noise model: unit (2) \n",
            "  EssentialMatrixFactor2 with measurements\n",
            "  (480 288   1)' and (464 312)'\n",
            "  EssentialMatrixFactor3 with rotation [\n",
            "\t0.99875, -0.0499792, 0;\n",
            "\t0.0499792, 0.99875, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "Error for Factor 3: 413.0638991792357\n"
          ]
        }
      ],
      "source": [
        "body_R_cam = Rot3.Yaw(0.05) # Example fixed rotation\n",
        "\n",
        "factor3 = EssentialMatrixFactor3(keyE, keyD, pA_pixels, pB_pixels, body_R_cam, reprojection_noise)\n",
        "factor3.print(\"\\nFactor 3: \")\n",
        "\n",
        "# Evaluate error (uses same E and d from values)\n",
        "error3 = factor3.error(values)\n",
        "print(f\"\\nError for Factor 3: {error3}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor4_header_md"
      },
      "source": [
        "## 4. `EssentialMatrixFactor4<CALIBRATION>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor4_desc_md"
      },
      "source": [
        "This factor involves an `EssentialMatrix` variable and a single unknown `CALIBRATION` variable (e.g., `Cal3_S2`) that is assumed to be **shared** by both cameras.\n",
        "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n",
        "The error is the algebraic epipolar error $ (K^{-1} p_B)^T E (K^{-1} p_A) $.\n",
        "\n",
        "**Note:** Recovering calibration from 2D correspondences alone is often ill-posed. This factor typically requires strong priors on the calibration."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {
        "id": "factor4_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Factor 4:   keys = { e0 k0 }\n",
            "isotropic dim=1 sigma=0.01\n",
            "  EssentialMatrixFactor4 with measurements\n",
            "  (480 288)' and (464 312)'\n",
            "\n",
            "Error for Factor 4: 11.520000000000007\n"
          ]
        }
      ],
      "source": [
        "# Key for the unknown shared Calibration\n",
        "keyK = K(0)\n",
        "\n",
        "factor4 = EssentialMatrixFactor4Cal3_S2(keyE, keyK, pA_pixels, pB_pixels, epipolar_noise)\n",
        "factor4.print(\"\\nFactor 4: \")\n",
        "\n",
        "# Evaluate error (requires E and K)\n",
        "values.insert(keyK, K_cal) # Use the known K for this example\n",
        "error4 = factor4.error(values)\n",
        "print(f\"\\nError for Factor 4: {error4}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor5_header_md"
      },
      "source": [
        "## 5. `EssentialMatrixFactor5<CALIBRATION>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor5_desc_md"
      },
      "source": [
        "Similar to Factor4, but allows for **two different** unknown `CALIBRATION` variables, one for each camera ($K_A$ and $K_B$).\n",
        "It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n",
        "The error is the algebraic epipolar error $ (K_B^{-1} p_B)^T E (K_A^{-1} p_A) $.\n",
        "\n",
        "**Note:** Like Factor4, this is often ill-posed without strong priors."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "metadata": {
        "id": "factor5_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Factor 5:   keys = { e0 k0 k1 }\n",
            "isotropic dim=1 sigma=0.01\n",
            "  EssentialMatrixFactor5 with measurements\n",
            "  (480 288)' and (464 312)'\n",
            "\n",
            "Error for Factor 5: 11.520000000000007\n"
          ]
        }
      ],
      "source": [
        "# Keys for potentially different calibrations\n",
        "keyKA = K(0) # Can reuse keyK if they are actually the same\n",
        "keyKB = K(1)\n",
        "\n",
        "factor5 = EssentialMatrixFactor5Cal3_S2(keyE, keyKA, keyKB, pA_pixels, pB_pixels, epipolar_noise)\n",
        "factor5.print(\"\\nFactor 5: \")\n",
        "\n",
        "# Evaluate error (requires E, KA, KB)\n",
        "# values already contains E(0) and K(0)\n",
        "values.insert(keyKB, K_cal) # Assume KB is also the same known K\n",
        "error5 = factor5.error(values)\n",
        "print(f\"\\nError for Factor 5: {error5}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
